/*                                                                                                                       
 * Copyright (c) 2010, Willow Garage, Inc.                                                                               
 * All rights reserved.                                                                                                  
 *                                                                                                                       
 * Redistribution and use in source and binary forms, with or without                                                    
 * modification, are permitted provided that the following conditions are met:                                           
 *                                                                                                                       
 *     * Redistributions of source code must retain the above copyright                                                  
 *       notice, this list of conditions and the following disclaimer.                                                   
 *     * Redistributions in binary form must reproduce the above copyright                                               
 *       notice, this list of conditions and the following disclaimer in the                                             
 *       documentation and/or other materials provided with the distribution.                                            
 *     * Neither the name of the Willow Garage, Inc. nor the names of its                                                
 *       contributors may be used to endorse or promote products derived from                                            
 *       this software without specific prior written permission.                                                        
 *                                                                                                                       
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"                                           
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE                                             
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE                                            
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE                                              
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR                                                   
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF                                                  
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS                                              
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN                                               
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)                                               
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE                                            
 * POSSIBILITY OF SUCH DAMAGE.                                                                                           
 */
// Copy from r2Serial.cpp

// communicate via RS232 serial with a remote teensy controller. 
// publish estimated height measurement using ROS TOPIC, Float32.

//Thread main

#include "sonar.h"

using namespace std;

//Initialize serial port, return file descriptor
int serialInit(char * port, int baud)
{
	int BAUD = 0;
	int fd = -1;
	struct termios newtio;
	FILE *fp = NULL;

	//Open the serial port as a file descriptor for low level configuration
	// read/write, not controlling terminal for process,
	fd = open(port, O_RDWR | O_NOCTTY | O_NDELAY );
	if ( fd<0 )
	{
		ROS_ERROR("serialInit: Could not open serial device %s",port);
		return fd;
	}

	// set up new settings
	memset(&newtio, 0,sizeof(newtio));
	newtio.c_cflag =  CS8 | CLOCAL | CREAD;  //no parity, 1 stop bit

	newtio.c_iflag = IGNCR;    //ignore CR, other options off
	newtio.c_iflag |= IGNBRK;  //ignore break condition

	newtio.c_oflag = 0;        //all options off

	newtio.c_lflag = ICANON;     //process input as lines

	// activate new settings
	tcflush(fd, TCIFLUSH);
	//Look up appropriate baud rate constant
	switch (baud)
	{
		case 38400:
		default:
			BAUD = B38400;
		break;
		
		case 19200:
			BAUD  = B19200;
		break;
		
		case 9600:
			BAUD  = B9600;
		break;
		
		case 4800:
			BAUD  = B4800;
		break;
		
		case 2400:
			BAUD  = B2400;
		break;
		
		case 1800:
			BAUD  = B1800;
		break;
		
		case 1200:
			BAUD  = B1200;
		break;
	}  //end of switch baud_rate
	if (cfsetispeed(&newtio, BAUD) < 0 || cfsetospeed(&newtio, BAUD) < 0)
	{
		ROS_ERROR("serialInit: Failed to set serial baud rate: %d", baud);
		close(fd);
		return NULL;
	}
	tcsetattr(fd, TCSANOW, &newtio);
	tcflush(fd, TCIOFLUSH);

	//Open file as a standard I/O stream
	fp = fdopen(fd, "r+");
	if (!fp) 
	{
		ROS_ERROR("serialInit: Failed to open serial stream %s", port);
		fp = NULL;
	}
	//return fp;
	if (!fd) 
	{
		ROS_ERROR("serialInit: Failed to open serial stream %s", port);
		fd = NULL;
	}
	return fd;

} //serialInit


//Process ROS command message, send to uController
void ucCommandCallback(const std_msgs::String::ConstPtr& msg)
{
	ROS_DEBUG("uc%dCommand: %s", ucIndex, msg->data.c_str());
	fprintf(fpSerial, "%s", msg->data.c_str()); //appends newline
} //ucCommandCallback


// Simple Low pass filter implementation

float LPF(float distance)
{
	 // Initialization for preDistance value
	 if(!First)
	 {
		 preDistance = distance;
		 First=1;
	 }
	 double lpf_distance = ALPHA*preDistance+(1-ALPHA)*distance;
	 preDistance = lpf_distance;
	 return lpf_distance;
}
//Receive command responses from robot uController
//and publish as a ROS message
void *rcvThread(void *arg)
{
	int rcvBufSize = 200;
	char ucResponse[rcvBufSize];   //response string from uController
	char *bufPos;
	//std_msgs::String msg;
	std_msgs::Float32 msg;
	//std_msgs::Float64 msg;
	std::stringstream ss;

	window_size=3;
	avg=0;
	initialized=0;

	ros::Rate loop_rate(40);
	ROS_INFO("rcvThread: receive thread running");
	float raw_distance=0,lpf_distance=0;

	//begin_time=ros::Time::now();
	cout<<"Waiting for the start command"<<endl;
	while (ros::ok()) 
	{


		int nLength;
		nLength = read(fdSerial,ucResponse,rcvBufSize);
		//printf("received length = %d %s\n",nLength,ucResponse);
			
		if (nLength>0) 
		{
			if(ucResponse[0]=='b')
			{
				//printf("Valid data received\n"); 
				//ROS_DEBUG("uc%dResponse: %s", ucIndex, ucResponse);
				//msg.data = ucResponse;
				char d[5];
				strcpy(d,(ucResponse+1));
				raw_distance=(float)atoi(d);
				//distance*=2.5*0.0254/9.765; // (in metre)
				raw_distance*=2.6*0.0254/9.765; // (in metre)
				height.data=raw_distance;
				ucResponseMsg.publish(height);
				
			}
		}

/*

		//if(quad_state==RECEIVE_START)
		{
		   	//if(!time_initialized)
			{
				begin_time=ros::Time::now();
				time_initialized=1;
			}
			current_time=ros::Time::now();
			// Strict thresholding filtering to get rid of peak.
			if(current_time.toSec()-begin_time.toSec() <5) // For 10 seconds at the beginning
			{
				if((raw_distance<HEIGHT_MAX) && (abs(avg-raw_distance)<BOUND))
				{
					if(avg_.size()<MOVING_AVG_WINDOW_SIZE)
					{
						avg_.push_back(raw_distance);
					}
					else
					{
						avg_.pop_front();
						avg_.push_back(raw_distance);
					}
					//sum.push_back(raw_distance);
					avg=accumulate(avg_.begin(),avg_.end(),0.0f)/avg_.size();
					cout<<"avg = "<<avg<< "  num size of avg_ = "<<avg_.size()<<endl;
					cout<<"Initial height= "<<raw_distance<<endl;
					height.data=raw_distance;
					ucResponseMsg.publish(height);

					fprintf(fd,"%f,%f\n",height.data,current_time.toSec());
				}
				else 
				{
					height.data=avg;
					ucResponseMsg.publish(height);
					cout<<"Reject peak using moving average = "<<avg<<endl;
				}
					
			}


			// Get rid of peak while flying.
			// Hope this work.
			else
			{
				if(abs(avg-raw_distance)<BOUND)
				{
					if(avg__.size()<MOVING_AVG_WINDOW_SIZE)
					{
						avg__.push_back(raw_distance);
					}
					else
					{
						avg__.pop_front();
						avg__.push_back(raw_distance);
					}
					avg=accumulate(avg__.begin(),avg__.end(),0.0f)/avg__.size();
					cout<<"avg = "<<avg<< "  num size of avg__ = "<<avg__.size()<<endl;
					height.data=raw_distance;
					cout<<"After height= "<<raw_distance<<endl;
					ucResponseMsg.publish(height);
					fprintf(fd,"%f,%f\n",height.data,current_time.toSec());
				}
				else
				{
					height.data=avg;
					ucResponseMsg.publish(height);
					cout<<"Reject peak using moving average = "<<avg<<endl;
				
				}
			}
		}
*/

		loop_rate.sleep();
  	}
  	return NULL;
} //rcvThread


int main(int argc, char **argv)
{
	char port[20];    //port name
	int baud;     //baud rate 

	char topicSubscribe[20];
	char topicPublish[20];

	pthread_t rcvThrID;   //receive thread ID
	int err;

	//Initialize ROS
	ros::init(argc, argv, "SonarReceiver");
	ros::NodeHandle rosNode;
	kbdSubscriber = rosNode.subscribe ("keycode", 100, &kbdCallback);
	ROS_INFO("SonarReceiver starting");

	fd=fopen("sonar_height","w");
	//fprintf(fd,"raw_distance,lpf_distance,predic,time\n");
	fprintf(fd,"height,time\n");

	//Open and initialize the serial port to the uController
	if (argc > 1) 
	{
		if(sscanf(argv[1],"%d", &ucIndex)==1) 
		{
			sprintf(topicSubscribe, "uc%dCommand",ucIndex);
			sprintf(topicPublish, "uc%dResponse",ucIndex);
		}
		else 
		{
			ROS_ERROR("ucontroller index parameter invalid");
			return 1;
		}
	}
	else 
	{
		strcpy(topicSubscribe, "uc0Command");
		strcpy(topicPublish, "height");
	}

	strcpy(port, DEFAULT_SERIALPORT);
	if (argc > 2) strcpy(port, argv[2]);

	baud = DEFAULT_BAUDRATE;
	if (argc > 3) 
	{
		if(sscanf(argv[3],"%d", &baud)!=1) 
		{
			ROS_ERROR("ucontroller baud rate parameter invalid");
			return 1;
		}
	}

	ROS_INFO("connection initializing (%s) at %d baud", port, baud);
	//fpSerial = serialInit(port, baud);
	fdSerial = serialInit(port, baud);
	if (!fdSerial )
	{
		ROS_ERROR("unable to create a new serial port");
		return 1;
	}
	ROS_INFO("serial connection successful");

	//Subscribe to ROS messages
	//ucCommandMsg = rosNode.subscribe(topicSubscribe, 100, ucCommandCallback);

	//Setup to publish ROS messages
	//ucResponseMsg = rosNode.advertise<std_msgs::String>(topicPublish, 100);
	ucResponseMsg = rosNode.advertise<std_msgs::Float32>(topicPublish, 100);

	//Create receive thread
	err = pthread_create(&rcvThrID, NULL, rcvThread, NULL);
	if (err != 0) 
	{
		ROS_ERROR("unable to create receive thread");
		return 1;
	}

	//Process ROS messages and send serial commands to uController
	ros::spin();

	//fclose(fdSerial);
	ROS_INFO("stopping");
	return 0;
}

void kbdCallback (const cyphy_kbd::KeyCode& msg)
{
		char data=msg.data;
		 ROS_INFO("key_value=0x%02x\n",data);
		 switch(data)
		 {
		      case KEYCODE_SPACEBAR: quad_state=RECEIVE_START;
		 }

}
